/*
 * @Author: LVGRAPE
 * @LastEditors: LVGRAPE
 */

#include "fc.h"
#include "maths.h"
#include "rc_controls.h"
#include "attitude.h"
#include "motor_mix.h"
#include <math.h>
#include "filters.h"
#include "anoPTv8_report.h"
#include "fc_state.h"
#include "position.h"

#define DBG_TAG "pid"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>

// Anti gravity I constant
#define AG_KI 21.586988f
#define D_LPF_RAW_SCALE 25
#define D_LPF_FILT_SCALE 22

#define MAX_PID_PROCESS_DENOM 16
#define PID_CONTROLLER_BETAFLIGHT 1
#define PID_SERVO_MIXER_SCALING 0.7f
#define PIDSUM_LIMIT 500
#define PIDSUM_LIMIT_YAW 400
#define PIDSUM_LIMIT_MIN 100
#define PIDSUM_LIMIT_MAX 1000
#define ITERM_RELAX_SETPOINT_THRESHOLD 40.0f
#define ITERM_RELAX_CUTOFF_DEFAULT 15
#define ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF 15 // The anti gravity throttle highpass filter cutoff
#define ANTI_GRAVITY_SMOOTH_FILTER_CUTOFF 3    // The anti gravity P smoothing filter cutoff
#define FALLTHROUGH __attribute__((fallthrough))

ZINO_CONFIG_REGISTER(struct pid_config, pid_config, 2, 0);

bool pid_config_reload_flag = false;
float yaw_heading_start = 0;
rollAndPitchTrims_t rollAndPitchTrims = {
    .values.roll = 0,
    .values.pitch = 0}; // 角度漂移补偿
const struct pid_config pid_params_defalut = {
    .pid = {
        [PID_ROLL] = {35, 10, 5, 0},
        [PID_PITCH] = {35, 10, 5, 0},
        [PID_YAW] = {20, 10, 0, 0},
        [PID_LEVEL] = {30, 0, 0, 0},
    },

    .yaw_lowpass_hz = 0,
    .dterm_notch_hz = 0,
    .dterm_notch_cutoff = 0,
    .dterm_lowpass_hz = 150,  // NOTE: dynamic lpf is enabled by default so this setting is actually
                              // overridden and the static lowpass 1 is disabled. We can't set this
                              // value to 0 otherwise Configurator versions 10.4 and earlier will also
                              // reset the lowpass filter type to PT1 overriding the desired BIQUAD setting.
    .dterm_lowpass2_hz = 150, // second Dterm LPF ON by default
};

pidProfile_t pidProfile1 = {
    .pid = {
        [PID_ROLL] = {42, 85, 35, 90},
        [PID_PITCH] = {46, 90, 38, 95},
        [PID_YAW] = {45, 90, 0, 90},
        [PID_LEVEL] = {50, 50, 75, 0},
    },
    .pidSumLimit = PIDSUM_LIMIT,
    .pidSumLimitYaw = PIDSUM_LIMIT_YAW,
    .yaw_lowpass_hz = 0,
    .dterm_notch_hz = 0,
    .dterm_notch_cutoff = 0,
    .itermWindupPointPercent = 99,
    .pidAtMinThrottle = PID_STABILISATION_ON,
    .levelAngleLimit = 55,
    .feedForwardTransition = 0,
    .yawRateAccelLimit = 0,
    .rateAccelLimit = 0,
    .itermThrottleThreshold = 250,
    .itermAcceleratorGain = 3500,
    .crash_time = 500,                        // ms
    .crash_delay = 0,                         // ms
    .crash_recovery_angle = 10,               // degrees
    .crash_recovery_rate = 100,               // degrees/second
    .crash_dthreshold = 50,                   // degrees/second/second
    .crash_gthreshold = 400,                  // degrees/second
    .crash_setpoint_threshold = 350,          // degrees/second
    .crash_recovery = PID_CRASH_RECOVERY_OFF, // off by default
    .horizon_tilt_effect = 75,
    .horizon_tilt_expert_mode = false,
    .crash_limit_yaw = 200,
    .itermLimit = 400,
    .throttle_boost = 5,
    .throttle_boost_cutoff = 15,
    .iterm_rotation = false,
    .iterm_relax = ITERM_RELAX_RP,
    .iterm_relax_cutoff = ITERM_RELAX_CUTOFF_DEFAULT,
    .iterm_relax_type = ITERM_RELAX_SETPOINT,
    .acro_trainer_angle_limit = 20,
    .acro_trainer_lookahead_ms = 50,
    .acro_trainer_debug_axis = ROLL,
    .acro_trainer_gain = 75,
    .abs_control_gain = 0,
    .abs_control_limit = 90,
    .abs_control_error_limit = 20,
    .abs_control_cutoff = 11,
    .antiGravityMode = ANTI_GRAVITY_SMOOTH,
    .dterm_lowpass_hz = 150,  // NOTE: dynamic lpf is enabled by default so this setting is actually
                              // overridden and the static lowpass 1 is disabled. We can't set this
                              // value to 0 otherwise Configurator versions 10.4 and earlier will also
                              // reset the lowpass filter type to PT1 overriding the desired BIQUAD setting.
    .dterm_lowpass2_hz = 150, // second Dterm LPF ON by default
    .dterm_filter_type = FILTER_PT1,
    .dterm_filter2_type = FILTER_PT1,
    .dyn_lpf_dterm_min_hz = 70,
    .dyn_lpf_dterm_max_hz = 170,
    .launchControlMode = LAUNCH_CONTROL_MODE_NORMAL,
    .launchControlThrottlePercent = 20,
    .launchControlAngleLimit = 0,
    .launchControlGain = 40,
    .launchControlAllowTriggerReset = true,
    .use_integrated_yaw = false,
    .integrated_yaw_relax = 200,
    .thrustLinearization = 0,
    .d_min = {23, 25, 0}, // roll, pitch, yaw
    .d_min_gain = 37,
    .d_min_advance = 20,
    .motor_output_limit = 100,
    .auto_profile_cell_count = AUTO_PROFILE_CELL_COUNT_STAY,
    .transient_throttle_limit = 0,
    .dyn_idle_min_rpm = 0,
    .dyn_idle_p_gain = 50,
    .dyn_idle_i_gain = 50,
    .dyn_idle_d_gain = 50,
    .dyn_idle_max_increase = 150,
    .ff_interpolate_sp = FF_INTERPOLATE_ON,
    .ff_max_rate_limit = 100,
    .ff_smooth_factor = 37,
    .ff_boost = 15,
    .dyn_lpf_curve_expo = 5,
    .level_race_mode = false,
    .vbat_sag_compensation = 0,
};

pidRuntime_t pidRuntime;
pidAxisData_t pidData[XYZ_AXIS_COUNT];
struct pidProfile_s *currentPidProfile;
bool ANGLE_MODE = 1;

typedef enum
{
    LEVEL_MODE_OFF = 0,
    LEVEL_MODE_R,
    LEVEL_MODE_RP,
} levelMode_e;
void attitude_pid_init(pidProfile_t *pidProfile)
{
    uint32_t targetPidLooptime = 1 * 1000; // us
    pidRuntime.dT = targetPidLooptime * 1e-6f;
    pidRuntime.pidFrequency = 1.0 / pidRuntime.dT;

    if (pidProfile->feedForwardTransition == 0)
    {
        pidRuntime.feedForwardTransition = 0;
    }
    else
    {
        pidRuntime.feedForwardTransition = 100.0f / pidProfile->feedForwardTransition;
    }
    for (int axis = ROLL; axis <= YAW; axis++)
    {
        pidRuntime.pidCoefficient[axis].Kp = PTERM_SCALE * pidProfile->pid[axis].P;
        pidRuntime.pidCoefficient[axis].Ki = ITERM_SCALE * pidProfile->pid[axis].I;
        pidRuntime.pidCoefficient[axis].Kd = DTERM_SCALE * pidProfile->pid[axis].D;
        // pidRuntime.pidCoefficient[axis].Kf = FEEDFORWARD_SCALE * (pidProfile->pid[axis].F / 100.0f);
    }
#ifdef USE_INTEGRATED_YAW_CONTROL
    if (!pidProfile->use_integrated_yaw)
#endif
    {
        pidRuntime.pidCoefficient[YAW].Ki *= 2.5f;
    }
    pidRuntime.levelGain = pidProfile->pid[PID_LEVEL].P / 10.0f;
    pidRuntime.horizonGain = pidProfile->pid[PID_LEVEL].I / 10.0f;
    pidRuntime.horizonTransition = (float)pidProfile->pid[PID_LEVEL].D;
    pidRuntime.horizonTiltExpertMode = pidProfile->horizon_tilt_expert_mode;
    pidRuntime.horizonCutoffDegrees = (175 - pidProfile->horizon_tilt_effect) * 1.8f;
    pidRuntime.horizonFactorRatio = (100 - pidProfile->horizon_tilt_effect) * 0.01f;
    pidRuntime.maxVelocity[ROLL] = pidRuntime.maxVelocity[PITCH] = pidProfile->rateAccelLimit * 100 * pidRuntime.dT;
    pidRuntime.maxVelocity[YAW] = pidProfile->yawRateAccelLimit * 100 * pidRuntime.dT;
    pidRuntime.itermWindupPointInv = 1.0f;
    if (pidProfile->itermWindupPointPercent < 100)
    {
        const float itermWindupPoint = pidProfile->itermWindupPointPercent / 100.0f;
        pidRuntime.itermWindupPointInv = 1.0f / (1.0f - itermWindupPoint);
    }
    pidRuntime.itermAcceleratorGain = pidProfile->itermAcceleratorGain;
    pidRuntime.crashTimeLimitUs = pidProfile->crash_time * 1000;
    pidRuntime.crashTimeDelayUs = pidProfile->crash_delay * 1000;
    pidRuntime.crashRecoveryAngleDeciDegrees = pidProfile->crash_recovery_angle * 10;
    pidRuntime.crashRecoveryRate = pidProfile->crash_recovery_rate;
    pidRuntime.crashGyroThreshold = pidProfile->crash_gthreshold;
    pidRuntime.crashDtermThreshold = pidProfile->crash_dthreshold;
    pidRuntime.crashSetpointThreshold = pidProfile->crash_setpoint_threshold;
    pidRuntime.crashLimitYaw = pidProfile->crash_limit_yaw;
    pidRuntime.itermLimit = pidProfile->itermLimit;
#if defined(USE_THROTTLE_BOOST)
    throttleBoost = pidProfile->throttle_boost * 0.1f;
#endif
    pidRuntime.itermRotation = pidProfile->iterm_rotation;
    pidRuntime.antiGravityMode = pidProfile->antiGravityMode;

    // Calculate the anti-gravity value that will trigger the OSD display.
    // For classic AG it's either 1.0 for off and > 1.0 for on.
    // For the new AG it's a continuous floating value so we want to trigger the OSD
    // display when it exceeds 25% of its possible range. This gives a useful indication
    // of AG activity without excessive display.
    pidRuntime.antiGravityOsdCutoff = 0.0f;
    if (pidRuntime.antiGravityMode == ANTI_GRAVITY_SMOOTH)
    {
        pidRuntime.antiGravityOsdCutoff += (pidRuntime.itermAcceleratorGain / 1000.0f) * 0.25f;
    }
    pidRuntime.antiGravityEnabled = 0;
    currentPidProfile = pidProfile;

    pt1FilterInit(&pidRuntime.antiGravityThrottleLpf, pt1FilterGain(ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF, pidRuntime.dT));
    pt1FilterInit(&pidRuntime.antiGravitySmoothLpf, pt1FilterGain(ANTI_GRAVITY_SMOOTH_FILTER_CUTOFF, pidRuntime.dT));

    const uint32_t pidFrequencyNyquist = pidRuntime.pidFrequency / 2; // No rounding needed
    uint16_t dTermNotchHz;
    if (pidProfile->dterm_notch_hz <= pidFrequencyNyquist)
    {
        dTermNotchHz = pidProfile->dterm_notch_hz;
    }
    else
    {
        if (pidProfile->dterm_notch_cutoff < pidFrequencyNyquist)
        {
            dTermNotchHz = pidFrequencyNyquist;
        }
        else
        {
            dTermNotchHz = 0;
        }
    }

    if (dTermNotchHz != 0 && pidProfile->dterm_notch_cutoff != 0)
    {
        pidRuntime.dtermNotchApplyFn = (filterApplyFnPtr)biquadFilterApply;
        const float notchQ = filterGetNotchQ(dTermNotchHz, pidProfile->dterm_notch_cutoff);
        for (int axis = ROLL; axis <= YAW; axis++)
        {
            biquadFilterInit(&pidRuntime.dtermNotch[axis], dTermNotchHz, targetPidLooptime, notchQ, FILTER_NOTCH);
        }
    }
    else
    {
        pidRuntime.dtermNotchApplyFn = nullFilterApply;
    }

    // 1st Dterm Lowpass Filter
    uint16_t dterm_lowpass_hz = pidProfile->dterm_lowpass_hz;
    if (dterm_lowpass_hz > 0 && dterm_lowpass_hz < pidFrequencyNyquist)
    {
        pidRuntime.dtermLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply;
        for (int axis = ROLL; axis <= YAW; axis++)
        {
            pt1FilterInit(&pidRuntime.dtermLowpass[axis].pt1Filter, pt1FilterGain(dterm_lowpass_hz, pidRuntime.dT));
        }
    }
    else
    {
        pidRuntime.dtermLowpassApplyFn = nullFilterApply;
    }

    // 2nd Dterm Lowpass Filter
    if (pidProfile->dterm_lowpass2_hz == 0 || pidProfile->dterm_lowpass2_hz > pidFrequencyNyquist)
    {
        pidRuntime.dtermLowpass2ApplyFn = nullFilterApply;
    }
    else
    {
        pidRuntime.dtermLowpass2ApplyFn = (filterApplyFnPtr)pt1FilterApply;
        for (int axis = ROLL; axis <= YAW; axis++)
        {
            pt1FilterInit(&pidRuntime.dtermLowpass2[axis].pt1Filter, pt1FilterGain(pidProfile->dterm_lowpass2_hz, pidRuntime.dT));
        }
    }

    if (pidProfile->yaw_lowpass_hz == 0 || pidProfile->yaw_lowpass_hz > pidFrequencyNyquist)
    {
        pidRuntime.ptermYawLowpassApplyFn = nullFilterApply;
    }
    else
    {
        pidRuntime.ptermYawLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply;
        pt1FilterInit(&pidRuntime.ptermYawLowpass, pt1FilterGain(pidProfile->yaw_lowpass_hz, pidRuntime.dT));
    }
}
void pidUpdateAntiGravityThrottleFilter(float throttle)
{
    if (pidRuntime.antiGravityMode == ANTI_GRAVITY_SMOOTH)
    {
        // calculate a boost factor for P in the same way as for I when throttle changes quickly
        const float antiGravityThrottleLpf = pt1FilterApply(&pidRuntime.antiGravityThrottleLpf, throttle);
        // focus P boost on low throttle range only
        if (throttle < 0.5f)
        {
            pidRuntime.antiGravityPBoost = 0.5f - throttle;
        }
        else
        {
            pidRuntime.antiGravityPBoost = 0.0f;
        }
        // use lowpass to identify start of a throttle up, use this to reduce boost at start by half
        if (antiGravityThrottleLpf < throttle)
        {
            pidRuntime.antiGravityPBoost *= 0.5f;
        }
        // high-passed throttle focuses boost on faster throttle changes
        pidRuntime.antiGravityThrottleHpf = fabsf(throttle - antiGravityThrottleLpf);
        pidRuntime.antiGravityPBoost = pidRuntime.antiGravityPBoost * pidRuntime.antiGravityThrottleHpf;
        // smooth the P boost at 3hz to remove the jagged edges and prolong the effect after throttle stops
        pidRuntime.antiGravityPBoost = pt1FilterApply(&pidRuntime.antiGravitySmoothLpf, pidRuntime.antiGravityPBoost);
    }
}
/**
 * @brief calculates strength of horizon leveling; 0 = none, 1.0 = most leveling
 * 摇杆不动时，强度为1，摇杆最大偏移时，强度为0
 * 倾斜角度越大，强度越低
 * @return float
 */
float calcHorizonLevelStrength(void)
{
    // start with 1.0 at center stick, 0.0 at max stick deflection:
    float horizonLevelStrength = 1.0f - MAX(getRcDeflectionAbs(ROLL), getRcDeflectionAbs(PITCH));

    // 0 at level, 90 at vertical, 180 at inverted (degrees):
    const float currentInclination = MAX(ABS(fc_sensor_data.attitude.roll), ABS(fc_sensor_data.attitude.pitch)) / 10.0f;

    // horizonTiltExpertMode:  0 = leveling always active when sticks centered,
    //                         1 = leveling can be totally off when inverted
    if (pidRuntime.horizonTiltExpertMode)
    {
        if (pidRuntime.horizonTransition > 0 && pidRuntime.horizonCutoffDegrees > 0)
        {
            // if d_level > 0 and horizonTiltEffect < 175
            // horizonCutoffDegrees: 0 to 125 => 270 to 90 (represents where leveling goes to zero)
            // inclinationLevelRatio (0.0 to 1.0) is smaller (less leveling)
            //  for larger inclinations; 0.0 at horizonCutoffDegrees value:
            const float inclinationLevelRatio = constrainf((pidRuntime.horizonCutoffDegrees - currentInclination) / pidRuntime.horizonCutoffDegrees, 0, 1);
            // apply configured horizon sensitivity:
            // when stick is near center (horizonLevelStrength ~= 1.0)
            //  H_sensitivity value has little effect,
            // when stick is deflected (horizonLevelStrength near 0.0)
            //  H_sensitivity value has more effect:
            horizonLevelStrength = (horizonLevelStrength - 1) * 100 / pidRuntime.horizonTransition + 1;
            // apply inclination ratio, which may lower leveling
            //  to zero regardless of stick position:
            horizonLevelStrength *= inclinationLevelRatio;
        }
        else
        { // d_level=0 or horizon_tilt_effect>=175 means no leveling
            horizonLevelStrength = 0;
        }
    }
    else
    { // horizon_tilt_expert_mode = 0 (leveling always active when sticks centered)
        float sensitFact;
        if (pidRuntime.horizonFactorRatio < 1.01f)
        { // if horizonTiltEffect > 0
            // horizonFactorRatio: 1.0 to 0.0 (larger means more leveling)
            // inclinationLevelRatio (0.0 to 1.0) is smaller (less leveling)
            //  for larger inclinations, goes to 1.0 at inclination==level:
            const float inclinationLevelRatio = (180 - currentInclination) / 180 * (1.0f - pidRuntime.horizonFactorRatio) + pidRuntime.horizonFactorRatio;
            // apply ratio to configured horizon sensitivity:
            sensitFact = pidRuntime.horizonTransition * inclinationLevelRatio;
        }
        else
        { // horizonTiltEffect=0 for "old" functionality
            sensitFact = pidRuntime.horizonTransition;
        }

        if (sensitFact <= 0)
        { // zero means no leveling
            horizonLevelStrength = 0;
        }
        else
        {
            // when stick is near center (horizonLevelStrength ~= 1.0)
            //  sensitFact value has little effect,
            // when stick is deflected (horizonLevelStrength near 0.0)
            //  sensitFact value has more effect:
            horizonLevelStrength = ((horizonLevelStrength - 1) * (100 / sensitFact)) + 1;
        }
    }
    return constrainf(horizonLevelStrength, 0, 1);
}

float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, float currentPidSetpoint)
{
    // calculate error angle and limit the angle to the max inclination
    // rcDeflection is in range [-1.0, 1.0]
    // float angle = getRcDeflection(axis) * rc_control_max()->level;
    float angle = fc_setpoint.angle.axis[axis];

    // if(axis == PITCH)
    // {
        // rt_kprintf_float("PITCH: ", angle, "\n");
        // rt_kprintf_float("ROLL: ", angle, "\n");
    // }

    angle = constrainf(angle, -pidProfile->levelAngleLimit, pidProfile->levelAngleLimit);
    const float errorAngle = angle - (fc_sensor_data.attitude.axis[axis] - angleTrim->raw[axis]);
    if (ANGLE_MODE)
    {
        currentPidSetpoint = errorAngle * pidRuntime.levelGain;
        // ANGLE mode - control is angle based
    }
    else
    {
        // HORIZON mode - mix of ANGLE and ACRO modes
        // mix in errorAngle to currentPidSetpoint to add a little auto-level feel
        const float horizonLevelStrength = calcHorizonLevelStrength();
        currentPidSetpoint = currentPidSetpoint + (errorAngle * pidRuntime.horizonGain * horizonLevelStrength);
    }
    return currentPidSetpoint;
}
#define USE_ACC 1
void pidController(const pidProfile_t *pidProfile, timeUs_t currentTimeUs)
{
    static float previousGyroRateDterm[XYZ_AXIS_COUNT];

#ifdef USE_INTERPOLATED_SP
    static FAST_DATA_ZERO_INIT uint32_t lastFrameNumber;
#endif
    // static float previousRawGyroRateDterm[XYZ_AXIS_COUNT];

#if defined(USE_ACC)
    static timeUs_t levelModeStartTimeUs = 0;
    // static bool gpsRescuePreviousState = false;
#endif

    // const float tpaFactor = getThrottlePIDAttenuation();
    const float tpaFactor = 1;

#if defined(USE_ACC)
    const rollAndPitchTrims_t *angleTrim = &rollAndPitchTrims;
#else
    UNUSED(pidProfile);
    UNUSED(currentTimeUs);
#endif

#ifdef USE_TPA_MODE
    const float tpaFactorKp = (currentControlRateProfile->tpaMode == TPA_MODE_PD) ? tpaFactor : 1.0f;
#else
    const float tpaFactorKp = 1.f;
#endif

#ifdef USE_YAW_SPIN_RECOVERY
    const bool yawSpinActive = gyroYawSpinDetected();
#endif

    const bool launchControlActive = 0;

#if defined(USE_ACC)

    levelMode_e levelMode;
    if (ANGLE_MODE)
    {
        if (pidRuntime.levelRaceMode)
        {
            levelMode = LEVEL_MODE_R;
        }
        else
        {
            levelMode = LEVEL_MODE_RP; // NOTE using this mode
        }
    }
    else
    {
        levelMode = LEVEL_MODE_OFF;
    }
    // LOG_D("levelMode = %d", levelMode);
    // Keep track of when we entered a self-level mode so that we can
    // add a guard time before crash recovery can activate.
    // Also reset the guard time whenever GPS Rescue is activated.
    if (levelMode)
    {
        if ((levelModeStartTimeUs == 0))
        {
            levelModeStartTimeUs = currentTimeUs;
        }
    }
    else
    {
        levelModeStartTimeUs = 0;
    }

#endif

    // LOG_D("antiGravityEnabled:%d", pidRuntime.antiGravityEnabled);
    // Dynamic i component,
    if ((pidRuntime.antiGravityMode == ANTI_GRAVITY_SMOOTH) && pidRuntime.antiGravityEnabled) // NOT enable
    {
        // traditional itermAccelerator factor for iTerm
        pidRuntime.itermAccelerator = pidRuntime.antiGravityThrottleHpf * 0.01f * pidRuntime.itermAcceleratorGain;
        // DEBUG_SET(DEBUG_ANTI_GRAVITY, 1, lrintf(pidRuntime.itermAccelerator * 1000));
        // users AG Gain changes P boost
        pidRuntime.antiGravityPBoost *= pidRuntime.itermAcceleratorGain;
        // add some percentage of that slower, longer acting P boost factor to prolong AG effect on iTerm
        pidRuntime.itermAccelerator += pidRuntime.antiGravityPBoost * 0.05f;
        // set the final P boost amount
        pidRuntime.antiGravityPBoost *= 0.02f;
    }
    else
    {
        pidRuntime.antiGravityPBoost = 0.0f;
    }
    // DEBUG_SET(DEBUG_ANTI_GRAVITY, 0, lrintf(pidRuntime.itermAccelerator * 1000));

    float agGain = pidRuntime.dT * pidRuntime.itermAccelerator * AG_KI; // NOTE Always 0

    // gradually scale back integration when above windup point
    float dynCi = pidRuntime.dT;
    if (pidRuntime.itermWindupPointInv > 1.0f)
    {
        dynCi *= constrainf((1.0f - getMotorMixRange()) * pidRuntime.itermWindupPointInv, 0.0f, 1.0f);
    }

    // Precalculate gyro deta for D-term here, this allows loop unrolling
    float gyroRateDterm[XYZ_AXIS_COUNT];
    for (int axis = ROLL; axis <= YAW; ++axis)
    {
        gyroRateDterm[axis] = fc_sensor_data.gyro.gyrolpf.axis[axis];
        // -----calculate raw, unfiltered D component

        // Divide rate change by dT to get differential (ie dr/dt).
        // dT is fixed and calculated from the target PID loop time
        // This is done to avoid DTerm spikes that occur with dynamically
        // calculated deltaT whenever another task causes the PID
        // loop execution to be delayed.

        // const float delta = -(gyroRateDterm[axis] - previousRawGyroRateDterm[axis]) * pidRuntime.pidFrequency / D_LPF_RAW_SCALE;
        // previousRawGyroRateDterm[axis] = gyroRateDterm[axis];

        // FIXME
        gyroRateDterm[axis] = pidRuntime.dtermNotchApplyFn((filter_t *)&pidRuntime.dtermNotch[axis], gyroRateDterm[axis]);
        gyroRateDterm[axis] = pidRuntime.dtermLowpassApplyFn((filter_t *)&pidRuntime.dtermLowpass[axis], gyroRateDterm[axis]);
        gyroRateDterm[axis] = pidRuntime.dtermLowpass2ApplyFn((filter_t *)&pidRuntime.dtermLowpass2[axis], gyroRateDterm[axis]);
    }

    // rotateItermAndAxisError();

    // ----------PID controller----------
    for (int axis = ROLL; axis <= YAW; ++axis)
    {

        float currentPidSetpoint = getSetpointRate(axis);
        // float currentPidSetpoint = 0;
        // if (pidRuntime.maxVelocity[axis])
        // {
        //     currentPidSetpoint = accelerationLimit(axis, currentPidSetpoint);
        // }
        // Yaw control is GYRO based, direct sticks control is applied to rate PID
        // When Race Mode is active PITCH control is also GYRO based in level or horizon mode
#if defined(USE_ACC)
        switch (levelMode)
        {
        case LEVEL_MODE_OFF:

            break;
        case LEVEL_MODE_R:
            if (axis == PITCH)
            {
                break;
            }

            FALLTHROUGH;
        case LEVEL_MODE_RP:
            if (axis == YAW)
            {
                currentPidSetpoint = getSetpointRate(axis) *0.001f * rc_control_max()->yaw;
                break;
            }
            currentPidSetpoint = pidLevel(axis, pidProfile, angleTrim, currentPidSetpoint);
        }
#endif

        // Handle yaw spin recovery - zero the setpoint on yaw to aid in recovery
        // It's not necessary to zero the set points for R/P because the PIDs will be zeroed below
#ifdef USE_YAW_SPIN_RECOVERY
        if ((axis == FD_YAW) && yawSpinActive)
        {
            currentPidSetpoint = 0.0f;
        }
#endif // USE_YAW_SPIN_RECOVERY

        // -----calculate error rate
        const float gyroRate = fc_sensor_data.gyro.gyrolpf.axis[axis]; // Process variable from gyro output in deg/sec
        float errorRate = currentPidSetpoint - gyroRate;               // r - y
#if defined(USE_ACC)
        // handleCrashRecovery(
        //     pidProfile->crash_recovery, angleTrim, axis, currentTimeUs, gyroRate,
        //     &currentPidSetpoint, &errorRate);
#endif

        const float previousIterm = pidData[axis].I;
        float itermErrorRate = errorRate;
#ifdef USE_ABSOLUTE_CONTROL
        float uncorrectedSetpoint = currentPidSetpoint;
#endif

#if defined(USE_ITERM_RELAX)
        if (!launchControlActive && !pidRuntime.inCrashRecoveryMode)
        {
            applyItermRelax(axis, previousIterm, gyroRate, &itermErrorRate, &currentPidSetpoint);
            errorRate = currentPidSetpoint - gyroRate;
        }
#endif
#ifdef USE_ABSOLUTE_CONTROL
        float setpointCorrection = currentPidSetpoint - uncorrectedSetpoint;
#endif

        // --------low-level gyro-based PID based on 2DOF PID controller. ----------
        // 2-DOF PID controller with optional filter on derivative term.
        // b = 1 and only c (feedforward weight) can be tuned (amount derivative on measurement or error).

        // -----calculate P component
        pidData[axis].P = pidRuntime.pidCoefficient[axis].Kp * errorRate * tpaFactorKp;
        if (axis == YAW)
        {
            pidData[axis].P = pidRuntime.ptermYawLowpassApplyFn((filter_t *)&pidRuntime.ptermYawLowpass, pidData[axis].P);


        }

        // -----calculate I component
        float Ki;
        float axisDynCi;

        Ki = pidRuntime.pidCoefficient[axis].Ki;
        axisDynCi = (axis == YAW) ? dynCi : pidRuntime.dT; // only apply windup protection to yaw

        pidData[axis].I = constrainf(previousIterm + (Ki * axisDynCi + agGain) * itermErrorRate, -pidRuntime.itermLimit, pidRuntime.itermLimit);

        // -----calculate pidSetpointDelta
        // float pidSetpointDelta = 0;

        // pidSetpointDelta = currentPidSetpoint - pidRuntime.previousPidSetpoint[axis];

        pidRuntime.previousPidSetpoint[axis] = currentPidSetpoint;

        // -----calculate D component
        // disable D if launch control is active
        if ((pidRuntime.pidCoefficient[axis].Kd > 0) && !launchControlActive)
        {

            // Divide rate change by dT to get differential (ie dr/dt).
            // dT is fixed and calculated from the target PID loop time
            // This is done to avoid DTerm spikes that occur with dynamically
            // calculated deltaT whenever another task causes the PID
            // loop execution to be delayed.
            const float delta = -(gyroRateDterm[axis] - previousGyroRateDterm[axis]) * pidRuntime.pidFrequency;
            float preTpaData = pidRuntime.pidCoefficient[axis].Kd * delta;
            pidData[axis].D = preTpaData * tpaFactor;
            // Log the value of D pre application of TPA
            preTpaData *= D_LPF_FILT_SCALE;
        }
        else
        {
            pidData[axis].D = 0;
        }

        previousGyroRateDterm[axis] = gyroRateDterm[axis];

        // -----calculate feedforward component
        // Only enable feedforward for rate mode and if launch control is inactive
        pidData[axis].F = 0;

        // calculating the PID sum
        // P boost at the end of throttle chop
        // attenuate effect if turning more than 50 deg/s, half at 100 deg/s
        float agBoostAttenuator = fabsf(currentPidSetpoint) / 50.0f;
        agBoostAttenuator = MAX(agBoostAttenuator, 1.0f);
        const float agBoost = 1.0f + (pidRuntime.antiGravityPBoost / agBoostAttenuator);
        pidData[axis].P *= agBoost;

        const float pidSum = pidData[axis].P + pidData[axis].I + pidData[axis].D + pidData[axis].F;
        {
            pidData[axis].Sum = pidSum;
        }
    }

    // Disable PID control if at zero throttle or if gyro overflow detected
    // This may look very innefficient, but it is done on purpose to always show real CPU usage as in flight
    // if (!pidRuntime.pidStabilisationEnabled)
    // {
    //     for (int axis = ROLL; axis <= YAW; ++axis)
    //     {
    //         pidData[axis].P = 0;
    //         pidData[axis].I = 0;
    //         pidData[axis].D = 0;
    //         pidData[axis].F = 0;

    //         pidData[axis].Sum = 0;
    //     }
    // }
    // else if (pidRuntime.zeroThrottleItermReset)
    // {
    //     // pidResetIterm();
    // }
    // rt_printf_float("dynci: ", dynCi, " \t");
    // rt_printf_float("Piddata Pitch: ", pidData[PITCH].Sum, " \t");
    // rt_printf_float("P: ", pidData[PITCH].P, " \t");
    // rt_printf_float("I: ", pidData[PITCH].I, " \t");
    // rt_printf_float("D: ", pidData[PITCH].D, " \n");
    if (!FC_STATE_CHECK(FC_ARMED))
    {
        pidData[ROLL].I = 0;
        pidData[PITCH].I = 0;
        pidData[YAW].I = 0;
    }

    //  rt_printf_float("yaw I: ", pidData[YAW].I, " \n");
}

void attiude_controler_init(void)
{
    if (pid_config()->flag != PID_SAVED_FLAG)
    {
        rt_memcpy(pid_config()->pid, pid_params_defalut.pid, sizeof(pid_params_defalut.pid));
        pid_config()->yaw_lowpass_hz = pid_params_defalut.yaw_lowpass_hz;
        pid_config()->dterm_notch_hz = pid_params_defalut.dterm_notch_hz;
        pid_config()->dterm_notch_cutoff = pid_params_defalut.dterm_notch_cutoff;
        pid_config()->dterm_lowpass_hz = pid_params_defalut.dterm_lowpass_hz;
        pid_config()->dterm_lowpass2_hz = pid_params_defalut.dterm_lowpass2_hz;
    }

    rt_memcpy(pidProfile1.pid, pid_config()->pid, sizeof(pidProfile1.pid));
    pidProfile1.yaw_lowpass_hz = pid_config()->yaw_lowpass_hz;
    pidProfile1.dterm_notch_hz = pid_config()->dterm_notch_hz;
    pidProfile1.dterm_notch_cutoff = pid_config()->dterm_notch_cutoff;
    pidProfile1.dterm_lowpass_hz = pid_config()->dterm_lowpass_hz;
    pidProfile1.dterm_lowpass2_hz = pid_config()->dterm_lowpass2_hz;

    // pidProfile1.levelAngleLimit =
    attitude_pid_init(&pidProfile1);

    LOG_D("ROLL:[%3d, %3d, %3d] \n", pidProfile1.pid[PID_ROLL].P, pidProfile1.pid[PID_ROLL].I, pidProfile1.pid[PID_ROLL].D);
    LOG_D("PITCH:[%3d, %3d, %3d] \n", pidProfile1.pid[PID_PITCH].P, pidProfile1.pid[PID_PITCH].I, pidProfile1.pid[PID_PITCH].D);
    LOG_D("YAW:[%3d, %3d, %3d] \n", pidProfile1.pid[PID_YAW].P, pidProfile1.pid[PID_YAW].I, pidProfile1.pid[PID_YAW].D);
    LOG_D("yaw_lowpass_hz: %d \n", pidProfile1.yaw_lowpass_hz);
    LOG_D("dterm_notch_hz: %d \n", pidProfile1.dterm_notch_hz);
    LOG_D("dterm_notch_cutoff: %d \n", pidProfile1.dterm_notch_cutoff);
    LOG_D("dterm_lowpass_hz: %d \n", pidProfile1.dterm_lowpass_hz);
    LOG_D("dterm_lowpass2_hz: %d \n", pidProfile1.dterm_lowpass2_hz);
}
void attitude_controler(void)
{
    static uint32_t previousTime = 0;
    uint32_t currentTime = rt_tick_get();
    uint32_t dt = currentTime - previousTime;
    pidRuntime.dT = dt * 0.001f;
    previousTime = currentTime;
    pidController(&pidProfile1, dt * 1000);
}

void save_pid_params(void)
{
    pid_config_reload_flag = true;
    pid_config()->flag = PID_SAVED_FLAG;
}

void pid_params_reload(void)
{
    if (pid_config_reload_flag)
    {
        if (pid_config()->flag == PID_SAVED_FLAG)
        {
            pid_config_reload_flag = false;
            rt_memcpy(pidProfile1.pid, pid_config()->pid, sizeof(pidProfile1.pid));
            LOG_D("pid_params_reload: \n");
            LOG_D("ROLL: [%3d, %3d, %3d]", pidProfile1.pid[PID_ROLL].P, pidProfile1.pid[PID_ROLL].I, pidProfile1.pid[PID_ROLL].D);
            LOG_D("PITCH:[%3d, %3d, %3d]", pidProfile1.pid[PID_PITCH].P, pidProfile1.pid[PID_PITCH].I, pidProfile1.pid[PID_PITCH].D);
            LOG_D("YAW:  [%3d, %3d, %3d]", pidProfile1.pid[PID_YAW].P, pidProfile1.pid[PID_YAW].I, pidProfile1.pid[PID_YAW].D);
            LOG_D("LEV:  [%3d, %3d, %3d]", pidProfile1.pid[PID_LEVEL].P, pidProfile1.pid[PID_LEVEL].I, pidProfile1.pid[PID_LEVEL].D);
            attitude_pid_init(&pidProfile1);
        }
    }
}
void pid_params_reset(void)
{
    rt_memcpy(pid_config()->pid, pid_params_defalut.pid, sizeof(pid_params_defalut.pid));
    pid_config()->yaw_lowpass_hz = pid_params_defalut.yaw_lowpass_hz;
    pid_config()->dterm_notch_hz = pid_params_defalut.dterm_notch_hz;
    pid_config()->dterm_notch_cutoff = pid_params_defalut.dterm_notch_cutoff;
    pid_config()->dterm_lowpass_hz = pid_params_defalut.dterm_lowpass_hz;
    pid_config()->dterm_lowpass2_hz = pid_params_defalut.dterm_lowpass2_hz;
}
